﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using UnityEngine;

namespace Assets.Scripts.Bvh.Common
{
    public class TraverseUtils
    {
        // 获得与线段相交的所有叶子结点，返回其包含的三角形
        // 注意，返回的三角形不一定都会与线段相交
        public static List<Triangle> GetIntersectingLeafNodes(RadixTreeNode[] LBvh, Vector3 lineStart, Vector3 lineEnd)
        {
            List<Triangle> triangles = new List<Triangle>();
            Stack<int> stack = new Stack<int>();
            //根节点入栈
            stack.Push(0);
            while (stack.Count > 0)
            {
                var nodeIndex = stack.Pop();
                var node = LBvh[nodeIndex];
                var aabb = node.aabb;
                bool isLeafNode = node.leftNodeIndex == -1 && node.rightNodeIndex == -1;
                bool isIntersecting = IsLineIntersectingAABB(lineStart, lineEnd, aabb.minCorner, aabb.maxCorner);
                if (isIntersecting)
                {
                    if (isLeafNode)
                    {
                        triangles.Add(node.triangle);
                    }
                    else
                    {
                        stack.Push(node.leftNodeIndex);
                        stack.Push(node.rightNodeIndex);
                    }
                }
            }
            return triangles;
        }

        // 检测线段是否与AABB相交  
        public static bool IsLineIntersectingAABB(Vector3 lineStart, Vector3 lineEnd, Vector3 aabbMin, Vector3 aabbMax)
        {
            // 用于比较浮点数的epsilon值  
            const float epsilon = 1e-3f;
            // 如果线段没有长度，则不能与AABB相交  
            if ((lineEnd - lineStart).sqrMagnitude <= 1e-6f)
            {
                return false;
            }

            // 分离轴定理（SAT）的实现  
            // 检查6个轴（AABB的3个边和线段的3个垂直方向）  
            Vector3[] axes = new Vector3[6];
            axes[0] = Vector3.right;
            axes[1] = Vector3.up;
            axes[2] = Vector3.forward;

            Vector3 lineDirection = (lineEnd - lineStart).normalized;
            axes[3] = Vector3.Cross(lineDirection, Vector3.right);
            axes[4] = Vector3.Cross(lineDirection, Vector3.up);
            axes[5] = Vector3.Cross(lineDirection, Vector3.forward);

            for (int i = 0; i < axes.Length; i++)
            {
                if (!IsOverlapOnAxis(lineStart, lineEnd, aabbMin, aabbMax, axes[i], epsilon))
                {
                    return false;
                }
            }

            return true;
        }

        // 检查在指定轴上是否有重叠  
        public static bool IsOverlapOnAxis(Vector3 lineStart, Vector3 lineEnd, Vector3 aabbMin, Vector3 aabbMax, Vector3 axis, float epsilon)
        {
            float lineMin = Vector3.Dot(lineStart, axis);
            float lineMax = Vector3.Dot(lineEnd, axis);

            // 确保lineMin <= lineMax  
            if (lineMin > lineMax)
            {
                float temp = lineMin;
                lineMin = lineMax;
                lineMax = temp;
            }

            float aabbMinProj = Vector3.Dot(aabbMin, axis);
            float aabbMaxProj = Vector3.Dot(aabbMax, axis);

            // 确保aabbMinProj <= aabbMaxProj  
            if (aabbMinProj > aabbMaxProj)
            {
                float temp = aabbMinProj;
                aabbMinProj = aabbMaxProj;
                aabbMaxProj = temp;
            }

            // 使用epsilon值来比较浮点数是否足够接近  
            if (lineMin > aabbMaxProj + epsilon || lineMax < aabbMinProj - epsilon)
            {
                return false;
            }

            return true;
        }


        // 计算三角形与线段的交点
        public static int GetLineTriangleIntersection(Vector3 lineStart, Vector3 lineEnd, Triangle triangle, out Vector3 intersectionPoint)
        {
            intersectionPoint = Vector3.zero; // 初始化交点为零向量

            // 计算三角形的法向量
            Vector3 normal = Vector3.Cross(triangle.v1 - triangle.v0, triangle.v2 - triangle.v0).normalized;

            // 计算线段的方向向量
            Vector3 direction = lineEnd - lineStart;

            // 计算线段起点到三角形平面的距离
            float distanceToPlane = Vector3.Dot(triangle.v0 - lineStart, normal);

            // 计算线段与平面法向量的点积，判断线段是否与平面平行
            float dotProduct = Vector3.Dot(direction, normal);

            // 如果线段与平面平行（或几乎平行），则不相交
            if (Mathf.Abs(dotProduct) < 1e-6f)
            {
                // 如果线段与平面重合，需要进一步检查线段是否在三角形内
                // （这里可以添加额外的逻辑，但通常这种情况可以视为不相交）
                return 0;
            }

            // 计算线段与平面的交点参数t
            float t = distanceToPlane / dotProduct;

            // 检查t是否在[0, 1]范围内，即交点是否在线段上
            if (t < 0f || t > 1f)
            {
                return 1; // 交点不在线段上
            }

            // 计算交点位置
            intersectionPoint = lineStart + t * direction;

            // 检查交点是否在三角形内部
            // 使用重心坐标法（Barycentric Coordinates）
            bool isValid = IsPointInTriangle(intersectionPoint, triangle.v0, triangle.v1, triangle.v2);
            if (isValid)
            {
                return 2;
            }

            return 3; // 交点在所处平面内但不在三角形内
        }

        // 使用重心坐标法（Barycentric Coordinates）检查交点是否在三角形内部
        public static bool IsPointInTriangle(Vector3 point, Vector3 p0, Vector3 p1, Vector3 p2)
        {
            // 一个非常小的容差值，用于处理浮点数精度问题  
            float epsilon = 1e-5f;

            // 计算向量  
            Vector3 v0 = p1 - p0;
            Vector3 v1 = p2 - p0;
            Vector3 v2 = point - p0;

            // 计算叉积  
            float dot00 = Vector3.Dot(v0, v0);
            float dot01 = Vector3.Dot(v0, v1);
            float dot02 = Vector3.Dot(v0, v2);
            float dot11 = Vector3.Dot(v1, v1);
            float dot12 = Vector3.Dot(v1, v2);

            // 计算重心坐标的分子和分母  
            float denom = dot00 * dot11 - dot01 * dot01;
            float u = (dot11 * dot02 - dot01 * dot12) / denom;
            float v = (dot00 * dot12 - dot01 * dot02) / denom;

            // 判断重心坐标是否在有效范围内，考虑epsilon容差值  
            return u >= -epsilon && v >= -epsilon && (u + v) <= 1 + epsilon;
        }
    }
}
